package es.uned.dia.james;
//import org.opensourcephysics.numerics.*;

public class PIDController {
    private LinearPlant plant;
    // feedback gains
    private double Kp, Ti, Td;
    // feedback thresholds
    private double deltaP, deltaI, deltaD;
    // feedforward gains
    private double[] Kff;
    // feedforward thresholds
    private double[] deltaFF;
    private double u, up, ui, ud, uff;
    // set point
    private double ysp, yspPrev, esp;
    private double ePrev, IEPrev, dePrev;
    
    private int order;
    // FSM: event, states and transitions
    private int state;
    
    public PIDController() {
    	init();
    }
    
    public void init() { 
    	plant = null;
    	Kp = Ti = Td = 0; Kff = null;
    	deltaP = deltaI = deltaD = 0; deltaFF = null;
        u = up = ui = ud = uff = 0;
        ysp = yspPrev = esp = 0;
        ePrev = IEPrev = dePrev = 0;  
        order = 1;
    	state = order;
    }
    
    public void setPIDParams(double Kp, double Ti, double Td) {
        setKp(Kp);
        setTi(Ti);
        setTd(Td);
    }
    
    public void setThresholds(double deltaP, double deltaI, double deltaD) {
        setDeltaP(deltaP);
        setDeltaI(deltaI);
        setDeltaD(deltaD);
    }
    
    public void setDeltaFF(double[] deltaFF) { if(deltaFF != null && deltaFF.length == order) for(int i=0; i<order; i++) this.deltaFF[i] = deltaFF[i]; }

    public void setKff(double[] Kff) { if(Kff != null && Kff.length == (order+1)) for(int i=0; i<=order; i++) this.Kff[i] = Kff[i]; }

    public void setKp(double Kp) { this.Kp = Kp; }
    
    public void setTi(double Ti) { this.Ti = Ti; }    
    
    public void setTd(double Td) { this.Td = Td; }

    public void setDeltaP(double deltaP) { this.deltaP = deltaP; }
    
    public void setDeltaI(double deltaI) { this.deltaI = deltaI; }
    
    public void setDeltaD(double deltaD) { this.deltaD = deltaD; }
    
    public boolean connect(LinearPlant plant) {
        if(plant == null) return false;
        
        this.plant = plant; 
        order = plant.states;
        state = order;
        Kff = new double[order+1];
        deltaFF = new double[order];
        return true;
    }

    public void event(TimeEvent ev) {        
        up = Kp*ev.e;
        ui += (Kp/Ti)*ev.IE;
        ud += (Kp*Td)*ev.de;
    }

    public void event(ProportionalEvent ev) {
        up = Kp*ev.e;        
    }

    public void event(IntegralEvent ev) {
        ui += Math.signum(ev.e)*(Kp/Ti)*ev.IE;
    }
    
    public void event(DerivativeEvent ev) {
        ud += (Kp*Td)*ev.de;
    }
    
    public void event(FeedforwardEvent ev) {     	
//        switch(state) {
//            case 0: /* uff = uff; */ break;
//            case 1: uff = Kff1 * esp; break;
//            case 2: uff = Kff2 * esp; break;
//            case 3: uff = ysp; break;
//            default: /* uff = uff; */ break;
//        }    	    	
        state = (state + 1) % (order + 1);
        uff = Kff[state];
    	System.out.println("FF event");
    	System.out.println("  state = " + state);
    	System.out.println("  Kff = " + Kff[state]);
    	if(state != order) System.out.println("  deltaFF = " + deltaFF[state]);
    }
    
/*    public double evaluateProportionalEvent(double e) {
        return Math.abs(e - ePrev) - deltaP;
    }
    
    public double evaluateIntegralEvent(double IE) {
        return Math.abs(IE) - deltaI;
    }
    
    public double evaluateDerivativeEvent(double de) {
        return Math.abs(de - dePrev) - deltaD;
    }
*/

    public double evaluate(EventInfo ei) {
    	if(state < 0 || state >= order) 
    		return (ei.ysp == this.ysp) ? 1.0 : - 1.0;
    	else
    		return ei.y - deltaFF[state];
    	
    }
    
    public double evaluateFeedforwardEvent(double y) {
    	if(state < 0 || state >= order) 
    		return 1.0;
    	else
    		return y - deltaFF[state];
    }

    public double u() {
        return uff + up + ui + ud;
    }
       
    public void setPoint(double t, double e, double ysp) {
    	if(state != order) return;
    	
//        event[0] = true;
        event(new FeedforwardEvent(t, e));
        this.esp = ysp - this.ysp;
        this.ysp = ysp;
    }
    
/*    public void setFFGains(double Kff1, double Kff2) {
        this.Kff1 = Kff1;
        this.Kff2 = Kff2;
    }
*/
 /*   public boolean findLevels(double e, double l) {
        SOPTD_ff ff = new SOPTD_ff();
        deltaFF = ff.findLevels(e, l, Kff1, Kff2);
        return true;
    }   
 */   
}

/*class SOPTD_ff implements Function {
    private double Kff1, Kff2;
    private double e, l;
    private double tol;
    
    public SOPTD_ff () {
        tol = 1e-2;
    }  
    
    public double evaluate(double t) {
        return Kff2 - Math.pow(Kff2 - Kff1*(1-Math.exp(-t)), (2*e - 1)/(e - 1)) / Math.pow(Kff2 - Kff1*(1-Math.exp(-t/e)), e/(e - 1)) - 1;
    }
       
    public double[] findLevels(double e, double l, double Kff1, double Kff2) {
        this.e = e;
        this.l = l;
        this.Kff1 = Kff1;
        this.Kff2 = Kff2;
        
        // Finds the normalized switching times (tau_1, tau_2)
//        double tau1 = Root.newton(this, 0.5, this.tol) - l;
        double tau1 = Root.bisection(this, 0, 10*(e+l), this.tol) - l;
        double tau2 = tau2(tau1);
        
        // Finds the normalized switching levels (delta_1, delta_2)
        double[] delta = {y1(tau1), y2(tau1, tau2)};        
        //Kff2 - Math.pow(Kff2 - Kff1*(1-Math.exp(-(tau1-l))), (2*e - 1)/(e - 1)) / Math.pow(Kff2 - Kff1*(1-Math.exp(-(t-l)/e)), e/(e - 1));
        
        return delta;
    }
    
    private double y1(double t) {
        return Kff1 * (1 - (1/(1-e))*Math.exp(-(t-l)) - (e/(e-1))*Math.exp(-(t-l)/e));            
    }   

    private double y2(double tau1, double tau2) {
        double ft1 = (Kff2 - Kff1*(1-Math.exp(-tau1)));
        double ft1oe = (Kff2 - Kff1*(1-Math.exp(-tau1/e)));        
        
        return Kff2 - (1/(1-e))*ft1*Math.exp(-(tau2-tau1-l)) - (e/(e-1))*ft1oe*Math.exp(-(tau2-tau1-l)/e);        
    }
    
    private double tau2(double t) {                
        return t + (e/(1-e))*Math.log((1-Math.exp(-t)) / (1-Math.exp(-t/e)));            
    }   
}*/